/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
// -*- coding:iso-8859-15 mode: c++ -*-
/** 
 * \file
 * \brief MaCI_Position Player for logged files.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * $Id: PositionFilePlayer.cpp,v 1.11 2009-11-13 15:11:18 morsko Exp $
 *
 */

#include "PositionFilePlayer.hpp"

PositionFilePlayer::PositionFilePlayer( MaCI::Position::CPositionServer *aPosServer,
                                        std::string aFileName)
  :PlayerModule(),
   iReader(),
   iPosServer(aPosServer),
   iData(),
   iTimes(),
   iBegin(0,0),
   iEnd(0,0),
   iFile(NULL),
   iName(""),
   iContainsTimestamp(false),
   iContainsPosition(false),
   iContainsVariance(false),
   iContainsProbability(false),
   iContainsSpeed(false)
{
 
  if(iReader.ReadFile(aFileName)){
    dPrint(3,"succesfully Opened file '%s'",aFileName.c_str());
  }else{
    dPrint(2,"Failed to open file '%s'",aFileName.c_str());
  }
  /*
  iFile= fopen(aFileName.c_str(),"rt");

  char c[200];
  char interfaceName[64];
  //Read the four first lines..and get the interfacename...
  fgets (c, 200,iFile );
 
  fscanf(iFile,"%s %s %s %s",c,c,c,interfaceName);
  fgets (c, 200,iFile );
  fgets (c, 200,iFile );
  //Look what datatypes are logged
  std::string types(c);
  std::string finalPrint = "Log file contains : ";
  std::string::size_type loc = types.find( "timestamp_sec", 0 );
  if( loc != std::string::npos ) {
    iContainsTimestamp=  true;
    finalPrint +="'timestamp' ";
  }

  loc = types.find("position_x",0);
  if( loc != std::string::npos ) {
    iContainsPosition=  true;
    finalPrint +="'position' ";
  }


  loc = types.find("variance_x",0);
  if( loc != std::string::npos ) {
     iContainsVariance=  true;
     finalPrint +="'variance' ";
  }
 

  loc = types.find("probability",0);
  if( loc != std::string::npos ) {
 
    iContainsProbability=  true;
    finalPrint += "'probability' ";
  }
   dPrint(1,"%s",c);

   loc = types.find("speed",0);
  if( loc != std::string::npos ) {
 
    iContainsSpeed=  true;
    finalPrint += "'speed' ";
  }
  finalPrint += "fields.";
  iPosServer->SetInterfaceInstanceName(std::string(interfaceName));
  iPosServer->Open();

  iName = "MaCI_Position."+std::string(interfaceName);
  dPrint(5,"%s",finalPrint.c_str());
  */
  ReadData();
  std::string instanceName;
  std::string macigroup;
  iReader.GetServiceInformation(macigroup,instanceName);
  iPosServer->SetInterfaceInstanceName(std::string(instanceName));
  iName = "MaCI_Position."+std::string(instanceName);
  iPosServer->Open();

}
//*****************************************************************************

PositionFilePlayer::~PositionFilePlayer()
{
  fclose(iFile);
  delete(iPosServer);
}
//*****************************************************************************

bool PositionFilePlayer::ReadData()
{

  if(iReader.GetTimes(iTimes)){
    if(iReader.GetAllData(iData)){
      dPrint(1,"Succesfully readed data. size %u %u",iTimes.size(),iData.size());
      iBegin = iTimes.front();
      iEnd = iTimes.back();
      

      
      return true;
    }
  }

  return false;
  /*    
  using namespace MaCI::Position;
  MaCI::Common::TTimestamp timestamp;
  float posX,posY,posA,varX,varY,varA,prob,speed,angularSpeed;
  int number = 1;
  bool first = true;
  if(!iContainsTimestamp){
    dPrint(3,"File doesn't contain timestamps. can't play data without timestamp");
    return false;
  }
  while(number != 0 && number != EOF){
    
    number = fscanf(iFile,"%u %u",
                    &(timestamp.timestamp_s),
                    &(timestamp.timestamp_us));
    if(first && number == 2){
      iBegin = timestamp.GetGimTime();
      first = false;
    }    

    if(iContainsPosition){
      number = fscanf(iFile,"%f %f %f",
                      &posX,&posY,&posA);
                   
      if(number != 3){
       
        continue;
      }
    }
    if(iContainsVariance){
      number = fscanf(iFile,"%f %f %f",
                      &varX,&varY,&varA);
      if(number != 3){
       
        continue;
      }
    }
    if(iContainsProbability){
      number = fscanf(iFile,"%f",
                      &prob);
      if(number != 1){
   
        continue;
      }
    }

    if(iContainsSpeed){
      number = fscanf(iFile,"%f %f",
                      &speed,&angularSpeed);
      if(number != 2){
   
        continue;
      }

    }
    if(number> 0){
      CPositionData *pos = new CPositionData();
      pos->CreateInternalBinBag();
      pos->SetTimestamp(timestamp);
      if(iContainsPosition){
        pos->SetPose2D(TPose2D(posX,posY,posA));
      }
      if(iContainsVariance){
        pos->SetVariance2D(TVariance2D(varX,varY,varA));
      }

      if(iContainsProbability){
        pos->SetProbability(TProbability(prob));
      }
      if(iContainsSpeed){
        pos->SetSpeed2D(TSpeed2D(speed,angularSpeed));
      }
      //pos->Print(1);
      iData[timestamp.GetGimTime()] = pos;
      iTimes.push_back(timestamp.GetGimTime());
    }

    

  }
  if(!first){
    iEnd = timestamp.GetGimTime();
  }
  if(number == EOF){
    return true;
  }else{
    return false;
  }
  */

}
//*****************************************************************************

bool PositionFilePlayer::OnTime(gim::time aTime)
{
  std::map<gim::time,MaCI::Position::CPositionData *>::iterator iter
    =iData.find(aTime);
  
  if(iter != iData.end()){
    iPosServer->SendPositionEvent(*(iter->second));
    return true;
  }else{
    
    return false;
  }
  
}
//*****************************************************************************

bool PositionFilePlayer::GetTimes(std::list<gim::time> *aTimes)
{
  
  aTimes->assign(iTimes.begin(),iTimes.end());
  return true;
}

//*****************************************************************************

std::string PositionFilePlayer::GetName(void)
{
  return iName;

}
//*****************************************************************************

void PositionFilePlayer::GetTimeLimits(gim::time &aBegin,gim::time &aEnd)
{
  aBegin = iBegin;
  aEnd = iEnd;
  dPrint(15,"begin %s, end %s",aBegin.toString().c_str(),aEnd.toString().c_str());
}
//*****************************************************************************







